﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace fhs
{
    class GtsMotionCard : GtsCard
    {
        struct AxisVel
        {
            public gts.mc.TTrapPrm param;
            public int vel;
        }
        AxisVel[] _AxisParams = new AxisVel[32];

        public GtsMotionCard(int card) : base(card)
        {

        }

        public override CardType Type
        {
            get
            {
                return CardType.Motion;
            }
        }

        public bool ServoOn(int axisNo)
        {
            lock (this)
            {
                return 0 == gts.mc.GT_AxisOn((short)axisNo) &&
                    0 == gts.mc.GT_ClrSts((short)axisNo, 1) &&
                    0 == gts.mc.GT_ZeroPos((short)axisNo, 1);
            }
        }

        public bool ServoOff(int axisNo)
        {
            lock (this)
            {
                return 0 == gts.mc.GT_AxisOff((short)axisNo);
            }
        }

        public bool Clear(int axisNo)
        {
            lock (this)
            {
                return 0 == gts.mc.GT_ClrSts((short)axisNo, 1);
            }
        }

        public bool Run(int axisNo, out bool run)
        {
            lock (this)
            {
                int state;
                uint clk = 0;
                if (0 == gts.mc.GT_GetSts((short)axisNo, out state, 1, out clk))
                {
                    run = 0 != (state & 0x400);
                    return true;
                }
                run = false;
                return false;
            }
        }

        public bool GetServoState(int axisNo, out int state)
        {
            lock (this)
            {
                uint clk = 0;
                return 0 == gts.mc.GT_GetSts((short)axisNo, out state, 1, out clk);
            }
        }

        public bool GetHomeState(int axisNo, out bool run, out bool error)
        {
            lock (this)
            {
                gts.mc.THomeStatus sts;
                if (0 == gts.mc.GT_GetHomeStatus((short)axisNo, out sts))
                {
                    run = 0 != sts.run;
                    error = 0 != sts.error;
                    return true;
                }
                run = error = false;
                return false;
            }
        }

        public bool ZeroPos(int axisNo)
        {
            lock (this)
            {
                return 0 == gts.mc.GT_ZeroPos((short)axisNo, 1);
            }
        }

        public bool PosError(int axisNo, out int pos)
        {
            lock (this)
            {
                uint clk = 0;
                double prfPos, encPos;
                if (0 == gts.mc.GT_GetPrfPos((short)axisNo, out prfPos, 1, out clk) &&
                    0 == gts.mc.GT_GetEncPos((short)axisNo, out encPos, 1, out clk))
                {
                    pos = (int)(prfPos - encPos);
                    return true;
                }
                pos = 0;
                return false;
            }
        }

        public bool GetDstPos(int axisNo, out int pos)
        {
            lock (this)
            {
                int dstPos;
                if (0 == gts.mc.GT_GetPos((short)axisNo, out dstPos))
                {
                    pos = dstPos;
                    return true;
                }
                pos = 0;
                return false;
            }
        }

        public bool GetPrfPos(int axisNo, out int pos)
        {
            lock (this)
            {
                uint clk = 0;
                double prfPos;
                if (0 == gts.mc.GT_GetPrfPos((short)axisNo, out prfPos, 1, out clk))
                {
                    pos = (int)prfPos;
                    return true;
                }
                pos = 0;
                return false;
            }
        }

        public bool GetEncPos(int axisNo, out int pos)
        {
            lock (this)
            {
                uint clk = 0;
                double encPos;
                if (0 == gts.mc.GT_GetEncPos((short)axisNo, out encPos, 1, out clk))
                {
                    pos = (int)encPos;
                    return true;
                }
                pos = 0;
                return false;
            }
        }

        public bool SetBand(int axisNo, int band, int time)
        {
            lock (this)
            {
                return 0 == gts.mc.GT_SetAxisBand((short)axisNo, band, time);
            }
        }

        public bool InitAxisParam(int axisNo)
        {
            lock (this)
            {
                return 0 == gts.mc.GT_GetTrapPrm((short)axisNo, out _AxisParams[axisNo].param);
            }
        }

        public void SetVel(int axisNo, int vel)
        {
            _AxisParams[axisNo].vel = vel;
        }

        public void SetAcc(int axisNo, double acc)
        {
            _AxisParams[axisNo].param.acc = acc;
        }

        public void SetDec(int axisNo, double dec)
        {
            _AxisParams[axisNo].param.dec = dec;
        }

        public void SetVelStart(int axisNo, int vel)
        {
            _AxisParams[axisNo].param.velStart = vel;
        }

        public void SetSmoothTime(int axisNo, int smoothTime)
        {
            _AxisParams[axisNo].param.smoothTime = (short)smoothTime;
        }

        public int GetVel(int axisNo)
        {
            return _AxisParams[axisNo].vel;
        }

        public double GetAcc(int axisNo)
        {
            return _AxisParams[axisNo].param.acc;
        }

        public double GetDec(int axisNo)
        {
            return _AxisParams[axisNo].param.dec;
        }

        public int GetVelStart(int axisNo)
        {
            return (int)_AxisParams[axisNo].param.velStart;
        }

        public int GetSmoothTime(int axisNo)
        {
            return (int)_AxisParams[axisNo].param.smoothTime;
        }

        public bool AbsMove(int axisNo, int pos)
        {
            lock (this)
            {
                return 0 == gts.mc.GT_PrfTrap((short)axisNo) &&
                    0 == gts.mc.GT_SetTrapPrm((short)axisNo, ref _AxisParams[axisNo].param) &&
                    0 == gts.mc.GT_SetVel((short)axisNo, _AxisParams[axisNo].vel) &&
                    0 == gts.mc.GT_SetPos((short)axisNo, pos) &&
                    0 == gts.mc.GT_Update(1 << (axisNo - 1));
            }
        }

        public bool RelativeMove(int axisNo, int pos)
        {
            lock (this)
            {
                int currPos = 0;
                return 0 == gts.mc.GT_PrfTrap((short)axisNo) &&
                    0 == gts.mc.GT_SetTrapPrm((short)axisNo, ref _AxisParams[axisNo].param) &&
                    0 == gts.mc.GT_SetVel((short)axisNo, _AxisParams[axisNo].vel) &&
                    0 == gts.mc.GT_GetPos((short)axisNo, out currPos) &&
                    0 == gts.mc.GT_SetPos((short)axisNo, currPos + pos) &&
                    0 == gts.mc.GT_Update(1 << (axisNo - 1));
            }
        }

        public bool JogMove(int axisNo, bool positive)
        {
            lock (this)
            {
                gts.mc.TJogPrm jogPrm = new gts.mc.TJogPrm { acc = _AxisParams[axisNo].param.acc, dec = _AxisParams[axisNo].param.dec, smooth = 0 };
                return 0 == gts.mc.GT_PrfJog((short)axisNo) &&
                    0 == gts.mc.GT_SetJogPrm((short)axisNo, ref jogPrm) &&
                    0 == gts.mc.GT_SetVel((short)axisNo, positive ? _AxisParams[axisNo].vel : -_AxisParams[axisNo].vel) &&
                    0 == gts.mc.GT_Update(1 << (axisNo - 1));
            }
        }

        public bool HomeMove(int axisNo, bool direction, int smooth, int vec, int offset, int distance)
        {
            lock (this)
            {
                gts.mc.THomePrm homePrm = new gts.mc.THomePrm();
                homePrm.mode = 20;
                homePrm.moveDir = direction ? (short)1 : (short)-1;
                homePrm.indexDir = direction ? (short)1 : (short)-1;
                homePrm.edge = 1;
                homePrm.smoothTime = (short)smooth;
                homePrm.velHigh = vec;
                homePrm.velLow = 0.1 * vec;
                homePrm.acc = 0.1;
                homePrm.dec = 0.1;
                homePrm.homeOffset = offset;
                homePrm.searchHomeDistance = distance;
                homePrm.searchIndexDistance = 0;
                homePrm.escapeStep = 1000;
                return 0 == gts.mc.GT_GoHome((short)axisNo, ref homePrm);
            }
        }

        public bool StopAxis(int axisNo, bool emg)
        {
            lock (this)
            {
                return 0 == gts.mc.GT_Stop(1 << (axisNo - 1), emg ? 1 << (axisNo - 1) : 0);
            }
        }

        public bool StopAll(bool emg)
        {
            lock (this)
            {
                return 0 == gts.mc.GT_Stop(0xFF, emg ? 0xFF : 0);
            }
        }

        public bool GetLimitPositive(int axisNo, out bool state)
        {
            lock (this)
            {
                int value;
                if (0 == gts.mc.GT_GetDi(gts.mc.MC_LIMIT_POSITIVE, out value))
                {
                    state = 0 != (value & (1 << (axisNo - 1)));
                    return true;
                }
                state = false;
                return false;
            }
        }

        public bool GetLimitNegative(int axisNo, out bool state)
        {
            lock (this)
            {
                int value;
                if (0 == gts.mc.GT_GetDi(gts.mc.MC_LIMIT_NEGATIVE, out value))
                {
                    state = 0 != (value & (1 << (axisNo - 1)));
                    return true;
                }
                state = false;
                return false;
            }
        }

        public bool GetHome(int axisNo, out bool state)
        {
            lock (this)
            {
                int value;
                if (0 == gts.mc.GT_GetDi(gts.mc.MC_HOME, out value))
                {
                    state = 0 != (value & (1 << (axisNo - 1)));
                    return true;
                }
                state = false;
                return false;
            }
        }

        public bool GetAlarm(int axisNo, out bool state)
        {
            lock (this)
            {
                int value;
                if (0 == gts.mc.GT_GetDi(gts.mc.MC_ALARM, out value))
                {
                    state = 0 != (value & (1 << (axisNo - 1)));
                    return true;
                }
                state = false;
                return false;
            }
        }

        public bool GetArrive(int axisNo, out bool state)
        {
            lock (this)
            {
                int value;
                if (0 == gts.mc.GT_GetDi(gts.mc.MC_ARRIVE, out value))
                {
                    state = 0 != (value & (1 << (axisNo - 1)));
                    return true;
                }
                state = false;
                return false;
            }
        }

        public bool GetIn(out int state)
        {
            lock (this)
            {
                return 0 == gts.mc.GT_GetDi(gts.mc.MC_GPI, out state);
            }
        }

        public bool GetOut(out int state)
        {
            lock (this)
            {
                return 0 == gts.mc.GT_GetDo(gts.mc.MC_GPO, out state);
            }
        }

        public bool SetDo(int state)
        {
            lock (this)
            {
                return 0 == gts.mc.GT_SetDo(gts.mc.MC_GPO, state);
            }
        }

        public bool SetDo(int index, bool state)
        {
            lock (this)
            {
                return 0 == gts.mc.GT_SetDoBit(gts.mc.MC_GPO, (short)index, state ? (short)1 : (short)0);
            }
        }
    }
}
